{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "EgiF12Hf1Dhs"
   },
   "source": [
    "# **Input and Output Ports of the Manipulation Station**\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "c2xGOvKCuQkd"
   },
   "source": [
    "## Notebook Setup \n",
    "The following cell will install Drake, checkout the manipulation repository, and set up the path (only if necessary).\n",
    "- On Google's Colaboratory, this **will take approximately two minutes** on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  \n",
    "\n",
    "More details are available [here](http://manipulation.mit.edu/drake.html)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "eeMrMI0-1Dhu"
   },
   "outputs": [],
   "source": [
    "import importlib\n",
    "import sys\n",
    "from urllib.request import urlretrieve\n",
    "\n",
    "server_args = []\n",
    "if 'google.colab' in sys.modules and importlib.util.find_spec('manipulation') is None:\n",
    "    urlretrieve(f\"http://manipulation.csail.mit.edu/scripts/setup/setup_manipulation_colab.py\",\n",
    "                \"setup_manipulation_colab.py\")\n",
    "    from setup_manipulation_colab import setup_manipulation\n",
    "    setup_manipulation(manipulation_sha='dc0eb8aa32e271d9b5299a84435694d0565346af', drake_version='20200909', drake_build='nightly')\n",
    "    server_args = ['--ngrok_http_tunnel']\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "P8HNbsnwSkzE"
   },
   "outputs": [],
   "source": [
    "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
    "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
    "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)\n",
    "\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "bClJCCHTKBb5"
   },
   "source": [
    "## Access System Input/Output Values\n",
    "In this exercise, you will explore the [ManipulationStation](https://drake.mit.edu/pydrake/pydrake.examples.manipulation_station.html?highlight=manipulationstation#pydrake.examples.manipulation_station.ManipulationStation) that was mentioned during the lecture. The system diagram is shown below. \n",
    "<img src=\"https://raw.githubusercontent.com/RussTedrake/manipulation/master/figures/exercises/manipulation_station_diagram.png\" width=\"1000\">\n",
    "\n",
    "You should recall that the orange ports are the ones that do not exist for the actual hardware platform.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "cR5vYf8Box0L"
   },
   "source": [
    "Now we construct a ManipulationStation object and finalize the system setting. To get a sense of what this manipulation station looks like, you can open the meshcat viewer from the generated link as usual. There should be a bookshelf and a Kuka arm with a gripper attached (it might take a few seconds to load)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "SN1HkYCy8VJo"
   },
   "outputs": [],
   "source": [
    "from pydrake.examples.manipulation_station import ManipulationStation\n",
    "from pydrake.systems.framework import DiagramBuilder\n",
    "from pydrake.systems.meshcat_visualizer import MeshcatVisualizer\n",
    "from pydrake.systems.analysis import Simulator\n",
    "\n",
    "station = ManipulationStation()\n",
    "station.SetupManipulationClassStation()\n",
    "plant = station.get_mutable_multibody_plant()\n",
    "station.Finalize()\n",
    "\n",
    "builder = DiagramBuilder()\n",
    "builder.AddSystem(station)\n",
    "scene_graph = station.get_mutable_scene_graph()\n",
    "meshcat = MeshcatVisualizer(scene_graph,\n",
    "            zmq_url=zmq_url)\n",
    "builder.AddSystem(meshcat)\n",
    "builder.Connect(station.GetOutputPort(\"pose_bundle\"),\n",
    "        meshcat.GetInputPort(\"lcm_visualization\"))\n",
    "diagram = builder.Build()\n",
    "simulator = Simulator(diagram)\n",
    "simulator.Initialize()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "82VsgQ938k7v"
   },
   "source": [
    "[**Context**](https://drake.mit.edu/pydrake/pydrake.systems.framework.html?highlight=context#pydrake.systems.framework.Context_) is an abstract class template that represents all the typed values that are used in a System\u2019s computations: time, numeric-valued input ports, numerical state, and numerical parameters. There are also type-erased abstract state variables, abstract-valued input ports, abstract parameters, and a double accuracy setting. It is important to note that a **Context** is designed to be used only with the System that created it. State and Parameter data can be copied between contexts for compatible systems as necessary. One of the most common mistakes is to pass the wrong context. Although most methods in drake should throw an error if you pass a context from the wrongsystem, but not all of them do yet. \n",
    "\n",
    "In the cell below, we first create a root context from the diagram, and then we retrieve the contexts of the subsystems from the root context."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "BS7uvKcIjwp0"
   },
   "outputs": [],
   "source": [
    "# initialize context\n",
    "context = diagram.CreateDefaultContext()\n",
    "plant_context = plant.GetMyContextFromRoot(context)\n",
    "station_context = station.GetMyContextFromRoot(context)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "_AuF4TWGlbFp"
   },
   "source": [
    "In this exercise, you will familiarize yourself with the input and output mechanism from the manipulation station system. Remember you can always generate a schematic view of your system by running the cell below. By clicking the \"+\" sign on the manipulation_station, you can get a more detailed view of the diverse modules within the manipulation station."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "OvSvFZH4_9Ni"
   },
   "outputs": [],
   "source": [
    "from IPython.display import HTML\n",
    "from pydrake.systems.framework import GenerateHtml\n",
    "\n",
    "diagram.set_name(\"diagram\")\n",
    "HTML('<script src=\"https://unpkg.com/gojs/release/go.js\"></script>' + GenerateHtml(diagram))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "FKDGlrCFqb15"
   },
   "source": [
    "Now if we set the joint position of the Kuka arm, we should expect to get the same values from the iiwa_position_measured port, which can be found from the output ports of **manipulation_station** object. Below we demonstrate how this can be done using **drake**'s syntax. You may also find it useful to review the **system** class documentation [here](https://drake.mit.edu/pydrake/pydrake.systems.framework.html?highlight=output_port#pydrake.systems.framework.System_)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "PwWvrx1_cPLl"
   },
   "outputs": [],
   "source": [
    "# provide initial states\n",
    "q0 = np.array([-1.57, 0.1, 0, -1.2, 0, 1.6, 0])\n",
    "# set the joint positions of the kuka arm\n",
    "station.SetIiwaPosition(station_context, q0)\n",
    "# examine the output port\n",
    "station.GetOutputPort(\"iiwa_position_measured\").Eval(station_context)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "8d7sH3kzrnYL"
   },
   "source": [
    "Note that the [output port](https://drake.mit.edu/pydrake/pydrake.systems.framework.html?highlight=outputport#pydrake.systems.framework.OutputPort) named \"iiwa_position_measured\" is first retrieved from the station and then evaluated using **Eval** method. This is a very common approach to read the values of a selected output port.\n",
    "\n",
    "Alternatively, you may retrieve the joint angles from the **plant**, which is a subsystem of the manipulation station."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "gbpZuVa_axTY"
   },
   "outputs": [],
   "source": [
    "joint_angles = []\n",
    "for i in range(1, 8):\n",
    "  joint_angles.append(\n",
    "  plant.GetJointByName('iiwa_joint_{}'.format(i)).get_angle(plant_context)\n",
    "  )\n",
    "\n",
    "# alternatively, use GetPositions to obtain the generalized positions\n",
    "# from the plant context\n",
    "q_general = plant.GetPositions(plant_context, \n",
    "                               plant.GetModelInstanceByName(\"iiwa\"))\n",
    "\n",
    "print(joint_angles)\n",
    "print(q_general)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "Ey-t3M_Usun3"
   },
   "source": [
    "# Exercise a: Code Submission\n",
    "Now, it's your time to code! Use **GetOutputPort** and **Eval** to retrieve the joint velocities from the \"iiwa_velocity_estimated\" output port. Note that we have set the velocities for you. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "A34DVaMRcVKV"
   },
   "outputs": [],
   "source": [
    "station.SetIiwaVelocity(station_context, np.zeros(7,))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "449kizGfUuwR"
   },
   "source": [
    "Below, `get_velocity(station, station_context)` is the function you must modify to query values from \"iiwa_velocity_estimated\"."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "RWa-mQwKRG4z"
   },
   "outputs": [],
   "source": [
    "def get_velocity(station, station_context):\n",
    "  \"\"\"\n",
    "  fill in your code in this method\n",
    "  \"\"\"\n",
    "  velocity_estimated =  None\n",
    "  return velocity_estimated"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "X8FLrTP-U3bH"
   },
   "source": [
    "You can check if you got the implementation correct by running the below autograder."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "fVShh5kyRG44"
   },
   "outputs": [],
   "source": [
    "from manipulation.exercises.robot.test_manipulation_io import TestManipulationIO\n",
    "from manipulation.exercises.grader import Grader\n",
    "\n",
    "Grader.grade_output([TestManipulationIO], [locals()], 'results.json')\n",
    "Grader.print_test_results('results.json')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "6dEYh81mtmWt"
   },
   "source": [
    "\"You may also command the Kuka joint positions via the [FixValue](https://drake.mit.edu/pydrake/pydrake.systems.framework.html?highlight=fixvalue#pydrake.systems.framework.InputPort) method. It fixes the inputs to the input port with selected values. An example is given below.  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "AXGR87MYdZkO"
   },
   "outputs": [],
   "source": [
    "q_command = np.array([-1.57, 0.5, 0, -1.2, 0, 1.6, 0])\n",
    "station.GetInputPort('iiwa_position').FixValue(station_context, q_command)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "TTPc4ssAuL8V"
   },
   "source": [
    "Now you should confirm by **GetOutputPort** and **Eval** that the \"iiwa_position_commanded\" port returns the same value as the q_command."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "tPJNE_-9evwU"
   },
   "outputs": [],
   "source": [
    "station.GetOutputPort(\"iiwa_position_commanded\").Eval(station_context)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "EEbQAd8SuvzQ"
   },
   "source": [
    "Please note that the *iiwa_position_commanded* and the *iiwa position* are NOT the same variable. The *iiwa_position_commanded* are the commanded positions sent to the robot, whereas the *iiwa_positions* are the current positions of the simulated robot. The input and output match perfectly in this case because of the luxury of simulation. However, the same observation can not be made from the feedforward torque (system input) and the commanded torque (system output). Next, you will investigate why. First, let us provide a zero feedforward torque to the \"iiwa_feedforward_torque\" port."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "gDV6v7YMlKzl"
   },
   "outputs": [],
   "source": [
    "station.GetInputPort(\"iiwa_feedforward_torque\").FixValue(station_context, np.zeros(7,))\n",
    "tau_no_ff = station.GetOutputPort(\"iiwa_torque_commanded\").Eval(station_context)\n",
    "print('feedforward torque: {}'.format(np.zeros(7,)))\n",
    "print('commanded torque with no feedforward torque:{}'.format(tau_no_ff))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "WDP0uoxtvtrq"
   },
   "source": [
    "Now try the same experiment with a non-zero feedforward torque as shown below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "zRxNOJKTfDbT"
   },
   "outputs": [],
   "source": [
    "tau_ff = np.linspace(3.1, 3.7, 7)\n",
    "print('feedforward torque: {}'.format(tau_ff))\n",
    "station.GetInputPort(\"iiwa_feedforward_torque\").FixValue(station_context, tau_ff)\n",
    "torque_commanded = station.GetOutputPort(\"iiwa_torque_commanded\").Eval(station_context)\n",
    "print('the commanded torque: {}'.format(torque_commanded))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "WKh2ETazw5-W"
   },
   "source": [
    "# Exercise b: Written Problem.\n",
    "Below, we have a question for you.\n",
    "\n",
    "**In this exercise, please explain what causes the discrepancy between the feedforward torque and the commanded torque.**\n",
    "\n",
    "HINT: can you find any relationship among *tau_ff*, *tau_no_ff*, *torque_commanded*?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "U2cjfMsITgVF"
   },
   "source": [
    "## Your Answer\n",
    "\n",
    "Answer the Question here, and copy-paste to the Gradescope 'written submission' section!\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "SCvUhPHMTqiH"
   },
   "source": [
    "## How will this notebook be graded?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "l5OAsgvJTt1M"
   },
   "source": [
    "If you are enrolled in the class, this notebook will be graded using [Gradescope](www.gradescope.com). You should have gotten the enrollment code on our announcement in Piazza.\n",
    "\n",
    "For submission of this assignment, you must do wo things. \n",
    "- Download and submit this notebook `manipulation_station_io.ipynb` to Gradescope's notebook submission section, along with your notebook for the first problem.\n",
    "- Copy and Paste your answer in the qualitative problem to Gradescope's written submission section. \n",
    "\n",
    "We will evaluate the local functions in the notebook to see if the function behaves as we have expected. For this exercise, the rubric is as follows:\n",
    "- [5pts] `get_velocity` must be implemented correctly. \n",
    "- [5pts] You must answer correctly why there is a difference between feed-forward torque and commanded torque. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "w48SCV0HUbcJ"
   },
   "source": [
    "## Additional Note."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "R324XG0CvLiN"
   },
   "source": [
    "So far we have not looked into the outputs of simulated camera sensors. Fortunately, accessing the camera data can be done in an almost exactly the same way as we have shown above. We will get to it soon! "
   ]
  }
 ],
 "metadata": {
  "celltoolbar": "Tags",
  "colab": {
   "collapsed_sections": [
    "c2xGOvKCuQkd"
   ],
   "name": "manipulation_station_io.ipynb",
   "provenance": [],
   "toc_visible": true
  },
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}